#include "Matrix.h"
#include "Factory.h"
#include <cassert>

namespace Engine
{
	Matrix::Matrix()
	{
		for(unsigned int n = 0; n < 16; ++n) m_matrix[n] = 0.0f;
	}

	Matrix::Matrix(const Quaternion& rhs)
	{
		m_matrix[ 0] = 1.0f - 2.0f * ( rhs.m_y * rhs.m_y + rhs.m_z * rhs.m_z ); 
		m_matrix[ 1] = 2.0f * (rhs.m_x * rhs.m_y + rhs.m_z * rhs.m_w);
		m_matrix[ 2] = 2.0f * (rhs.m_x * rhs.m_z - rhs.m_y * rhs.m_w);
		m_matrix[ 3] = 0.0f;  

		m_matrix[ 4] = 2.0f * ( rhs.m_x * rhs.m_y - rhs.m_z * rhs.m_w );  
		m_matrix[ 5] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_z * rhs.m_z ); 
		m_matrix[ 6] = 2.0f * (rhs.m_z * rhs.m_y + rhs.m_x * rhs.m_w );  
		m_matrix[ 7] = 0.0f;  

		m_matrix[ 8] = 2.0f * ( rhs.m_x * rhs.m_z + rhs.m_y * rhs.m_w );
		m_matrix[ 9] = 2.0f * ( rhs.m_y * rhs.m_z - rhs.m_x * rhs.m_w );
		m_matrix[10] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_y * rhs.m_y );  
		m_matrix[11] = 0.0f;  

		m_matrix[12] = 0;  
		m_matrix[13] = 0;  
		m_matrix[14] = 0;  
		m_matrix[15] = 1.0f;
	}

	Matrix& Matrix::operator=(const Quaternion& rhs)
	{
		m_matrix[ 0] = 1.0f - 2.0f * ( rhs.m_y * rhs.m_y + rhs.m_z * rhs.m_z ); 
		m_matrix[ 1] = 2.0f * (rhs.m_x * rhs.m_y + rhs.m_z * rhs.m_w);
		m_matrix[ 2] = 2.0f * (rhs.m_x * rhs.m_z - rhs.m_y * rhs.m_w);
		m_matrix[ 3] = 0.0f;  

		m_matrix[ 4] = 2.0f * ( rhs.m_x * rhs.m_y - rhs.m_z * rhs.m_w );  
		m_matrix[ 5] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_z * rhs.m_z ); 
		m_matrix[ 6] = 2.0f * (rhs.m_z * rhs.m_y + rhs.m_x * rhs.m_w );  
		m_matrix[ 7] = 0.0f;  

		m_matrix[ 8] = 2.0f * ( rhs.m_x * rhs.m_z + rhs.m_y * rhs.m_w );
		m_matrix[ 9] = 2.0f * ( rhs.m_y * rhs.m_z - rhs.m_x * rhs.m_w );
		m_matrix[10] = 1.0f - 2.0f * ( rhs.m_x * rhs.m_x + rhs.m_y * rhs.m_y );  
		m_matrix[11] = 0.0f;  

		m_matrix[12] = 0;  
		m_matrix[13] = 0;  
		m_matrix[14] = 0;  
		m_matrix[15] = 1.0f;

		return (*this);
	}

	void Matrix::SetToIdentity()
	{
		for(unsigned int n = 0; n < 16; ++n)
		{
			m_matrix[n] = 0.0f;
		}

		m_matrix[0] = m_matrix[5] = m_matrix[10] = m_matrix[15] = 1.0f;
	}

	Matrix& Matrix::operator*=(const Matrix& rhs)
	{
		float temp[16];

		temp[0] = (m_matrix[0]  * rhs.m_matrix[0])  +
			(m_matrix[4]  * rhs.m_matrix[1])  +
			(m_matrix[8]  * rhs.m_matrix[2])  +
			(m_matrix[12] * rhs.m_matrix[3]);

		temp[4] = (m_matrix[0]  * rhs.m_matrix[4])  +
			(m_matrix[4]  * rhs.m_matrix[5])  +
			(m_matrix[8]  * rhs.m_matrix[6])  +
			(m_matrix[12] * rhs.m_matrix[7]);

		temp[8] = (m_matrix[0]  * rhs.m_matrix[8])  +
			(m_matrix[4]  * rhs.m_matrix[9])  +
			(m_matrix[8]  * rhs.m_matrix[10]) +
			(m_matrix[12] * rhs.m_matrix[11]);

		temp[12] =(m_matrix[0]  * rhs.m_matrix[12]) +
			(m_matrix[4]  * rhs.m_matrix[13]) +
			(m_matrix[8]  * rhs.m_matrix[14]) +
			(m_matrix[12] * rhs.m_matrix[15]); 

		temp[1] = (m_matrix[1]  * rhs.m_matrix[0])  +
			(m_matrix[5]  * rhs.m_matrix[1])  +
			(m_matrix[9]  * rhs.m_matrix[2])  +
			(m_matrix[13] * rhs.m_matrix[3]);

		temp[5] = (m_matrix[1]  * rhs.m_matrix[4])  +
			(m_matrix[5]  * rhs.m_matrix[5])  +
			(m_matrix[9]  * rhs.m_matrix[6])  +
			(m_matrix[13] * rhs.m_matrix[7]);

		temp[9] = (m_matrix[1]  * rhs.m_matrix[8])  +
			(m_matrix[5]  * rhs.m_matrix[9])  +
			(m_matrix[9]  * rhs.m_matrix[10]) +
			(m_matrix[13] * rhs.m_matrix[11]);

		temp[13] =(m_matrix[1]  * rhs.m_matrix[12]) +
			(m_matrix[5]  * rhs.m_matrix[13]) +
			(m_matrix[9]  * rhs.m_matrix[14]) +
			(m_matrix[13] * rhs.m_matrix[15]); 

		temp[2] = (m_matrix[2]  * rhs.m_matrix[0])  +
			(m_matrix[6]  * rhs.m_matrix[1])  +
			(m_matrix[10] * rhs.m_matrix[2])  +
			(m_matrix[14] * rhs.m_matrix[3]);

		temp[6] = (m_matrix[2]  * rhs.m_matrix[4])  +
			(m_matrix[6]  * rhs.m_matrix[5])  +
			(m_matrix[10] * rhs.m_matrix[6])  +
			(m_matrix[14] * rhs.m_matrix[7]);

		temp[10]= (m_matrix[2]  * rhs.m_matrix[8])  +
			(m_matrix[6]  * rhs.m_matrix[9])  +
			(m_matrix[10] * rhs.m_matrix[10]) +
			(m_matrix[14] * rhs.m_matrix[11]);

		temp[14]= (m_matrix[2]  * rhs.m_matrix[12]) +
			(m_matrix[6]  * rhs.m_matrix[13]) +
			(m_matrix[10] * rhs.m_matrix[14]) +
			(m_matrix[14] * rhs.m_matrix[15]); 

		temp[3]= (m_matrix[3]  * rhs.m_matrix[0])  +
			(m_matrix[7]  * rhs.m_matrix[1])  +
			(m_matrix[11] * rhs.m_matrix[2])  +
			(m_matrix[15] * rhs.m_matrix[3]);

		temp[7]= (m_matrix[3]  * rhs.m_matrix[4])  +
			(m_matrix[7]  * rhs.m_matrix[5])  +
			(m_matrix[11] * rhs.m_matrix[6])  +
			(m_matrix[15] * rhs.m_matrix[7]);

		temp[11]= (m_matrix[3]  * rhs.m_matrix[8])  +
			(m_matrix[7]  * rhs.m_matrix[9])  +
			(m_matrix[11] * rhs.m_matrix[10]) +
			(m_matrix[15] * rhs.m_matrix[11]);

		temp[15]= (m_matrix[3]  * rhs.m_matrix[12]) +
			(m_matrix[7]  * rhs.m_matrix[13]) +
			(m_matrix[11] * rhs.m_matrix[14]) +
			(m_matrix[15] * rhs.m_matrix[15]); 

		for(unsigned int n = 0; n < 16; ++n)
		{
			m_matrix[n] = temp[n];
		}

		return (*this);
	}

	Matrix& Matrix::operator*=(const float* f)
	{
		float temp[16];

		temp[0] = (m_matrix[0]  * f[0])  +
			(m_matrix[4]  * f[1])  +
			(m_matrix[8]  * f[2])  +
			(m_matrix[12] * f[3]);

		temp[1] = (m_matrix[0]  * f[4])  +
			(m_matrix[4]  * f[5])  +
			(m_matrix[8]  * f[6])  +
			(m_matrix[12] * f[7]);

		temp[2] = (m_matrix[0]  * f[8])  +
			(m_matrix[4]  * f[9])  +
			(m_matrix[8]  * f[10]) +
			(m_matrix[12] * f[11]);

		temp[3] = (m_matrix[0]  * f[12]) +
			(m_matrix[4]  * f[13]) +
			(m_matrix[8]  * f[14]) +
			(m_matrix[12] * f[15]); 

		temp[4] = (m_matrix[1]  * f[0])  +
			(m_matrix[5]  * f[1])  +
			(m_matrix[9]  * f[2])  +
			(m_matrix[13] * f[3]);

		temp[5] = (m_matrix[1]  * f[4])  +
			(m_matrix[5]  * f[5])  +
			(m_matrix[9]  * f[6])  +
			(m_matrix[13] * f[7]);

		temp[6] = (m_matrix[1]  * f[8])  +
			(m_matrix[5]  * f[9])  +
			(m_matrix[9]  * f[10]) +
			(m_matrix[13] * f[11]);

		temp[7] = (m_matrix[1]  * f[12]) +
			(m_matrix[5]  * f[13]) +
			(m_matrix[9]  * f[14]) +
			(m_matrix[13] * f[15]); 

		temp[8] = (m_matrix[2]  * f[0])  +
			(m_matrix[6]  * f[1])  +
			(m_matrix[10] * f[2])  +
			(m_matrix[14] * f[3]);

		temp[9] = (m_matrix[2]  * f[4])  +
			(m_matrix[6]  * f[5])  +
			(m_matrix[10] * f[6])  +
			(m_matrix[14] * f[7]);

		temp[10]= (m_matrix[2]  * f[8])  +
			(m_matrix[6]  * f[9])  +
			(m_matrix[10] * f[10]) +
			(m_matrix[14] * f[11]);

		temp[11]= (m_matrix[2]  * f[12]) +
			(m_matrix[6]  * f[13]) +
			(m_matrix[10] * f[14]) +
			(m_matrix[14] * f[15]); 

		temp[12]= (m_matrix[3]  * f[0])  +
			(m_matrix[7]  * f[1])  +
			(m_matrix[11] * f[2])  +
			(m_matrix[15] * f[3]);

		temp[13]= (m_matrix[3]  * f[4])  +
			(m_matrix[7]  * f[5])  +
			(m_matrix[11] * f[6])  +
			(m_matrix[15] * f[7]);

		temp[14]= (m_matrix[3]  * f[8])  +
			(m_matrix[7]  * f[9])  +
			(m_matrix[11] * f[10]) +
			(m_matrix[15] * f[11]);

		temp[15]= (m_matrix[3]  * f[12]) +
			(m_matrix[7]  * f[13]) +
			(m_matrix[11] * f[14]) +
			(m_matrix[15] * f[15]); 

		for(unsigned int n = 0; n < 16; ++n)
		{
			m_matrix[n] = temp[n];
		}

		return (*this);
	}

	Matrix Matrix::operator*(const Matrix& rhs) const
	{
		Matrix temp = rhs;
		temp *= (*this);

		return temp;
	}

	Matrix Matrix::operator*(const float* f) const
	{
		Matrix temp = (*this);
		temp *= f;

		return temp;
	}

	Vec3 Matrix::operator*(const Vec3& f) const
	{
		Vec4 v4;

		v4.x = (m_matrix[0]  * f.x)  +
			(m_matrix[4]  * f.y)  +
			(m_matrix[8]  * f.z)  +
			(m_matrix[12]);

		v4.y = (m_matrix[1]  * f.x)  +
			(m_matrix[5]  * f.y)  +
			(m_matrix[9]  * f.z)  +
			(m_matrix[13]);

		v4.z = (m_matrix[2]  * f.x)  +
			(m_matrix[6]  * f.y)  +
			(m_matrix[10] * f.z)  +
			(m_matrix[14]);

		v4.w = (m_matrix[3]  * f.x)  +
			(m_matrix[7]  * f.y)  +
			(m_matrix[11] * f.z)  +
			(m_matrix[15]);

		return Vec3(v4.x / v4.w, v4.y / v4.w, v4.z / v4.w);
	}

	Engine::Vec4 Matrix::operator*( const Vec4& f) const
	{
		Vec4 v4;

		v4.x = (m_matrix[0]  * f.x)  +
			(m_matrix[4]  * f.y)  +
			(m_matrix[8]  * f.z)  +
			(m_matrix[12] * f.w);

		v4.y = (m_matrix[1]  * f.x)  +
			(m_matrix[5]  * f.y)  +
			(m_matrix[9]  * f.z)  +
			(m_matrix[13] * f.w);

		v4.z = (m_matrix[2]  * f.x)  +
			(m_matrix[6]  * f.y)  +
			(m_matrix[10] * f.z)  +
			(m_matrix[14] * f.w);

		v4.w = (m_matrix[3]  * f.x)  +
			(m_matrix[7]  * f.y)  +
			(m_matrix[11] * f.z)  +
			(m_matrix[15] * f.w);

		return v4;
	}

	void Matrix::SetTranslation(const Vec3& t)
	{
		m_matrix[12] = t.x;
		m_matrix[13] = t.y;
		m_matrix[14] = t.z;
		m_matrix[15] = 1.0f;
	}

	void Matrix::SetRotationX(float rads)
	{
		m_matrix[0] = 1.0f;
		m_matrix[5] = std::cos(rads);
		m_matrix[6] = std::sin(rads);
		m_matrix[9] = -std::sin(rads);
		m_matrix[10] = std::cos(rads);
		m_matrix[15] = 1.0f;
	}

	void Matrix::SetRotationY(float rads)
	{
		m_matrix[0] = std::cos(rads);
		m_matrix[2] = -std::sin(rads);
		m_matrix[5] = 1.0f;
		m_matrix[8] = std::sin(rads);
		m_matrix[10] = std::cos(rads);
		m_matrix[15] = 1.0f;
	}

	void Matrix::SetRotationZ(float rads)
	{
		m_matrix[0] = std::cos(rads);
		m_matrix[1] = std::sin(rads);
		m_matrix[4] = -std::sin(rads);
		m_matrix[5] = std::cos(rads);
		m_matrix[10] = 1.0f;
		m_matrix[15] = 1.0f;
	}

	void Matrix::SetScale(const Vec3& s)
	{
		m_matrix[0] = s.x;
		m_matrix[5] = s.y;
		m_matrix[10] = s.z;
		m_matrix[15] = 1.0f;
	}

	float& Matrix::operator[](unsigned int n)
	{
		assert(n < 16);
		return m_matrix[n];
	}

	void Matrix::SetToModelViewMatrix()
	{
		glGetFloatv(GL_MODELVIEW_MATRIX, m_matrix);
	}

	void Matrix::SetToProjectionMatrix()
	{
		glGetFloatv(GL_PROJECTION_MATRIX, m_matrix);
	}

}

